﻿using System;
using System.Diagnostics;

namespace RayDen.Library.Core.Primitives
{
    using Vec3f = Vector;

    public class ONB
    {

        public ONB()
        {
            mX = new Vec3f(1, 0, 0);
            mY = new Vec3f(0, 1, 0);
            mZ = new Vec3f(0, 0, 1);
        }

        public ONB(
            Vec3f x,
             Vec3f y,
             Vec3f z)
        {
            mX = x;
            mY = y;
            mZ = z;
        }

        public ONB(ref Vector v)
        {
            this.SetFromZ(ref v);
        }

        public ONB(ref Normal v)
        {
            this.SetFromZ(ref v);
        }

        public void SetFromZ(ref Vec3f z)
        {
            Vec3f tmpZ = mZ = Vector.Normalize(z);
            Vec3f tmpX = (Math.Abs(tmpZ.x) > 0.99f) ? new Vec3f(0, 1, 0) : new Vec3f(1, 0, 0);
            mY = Vector.Normalize(Vector.Cross(ref tmpZ, ref tmpX));
            mX = Vector.Cross(ref mY, ref tmpZ);
        }

        public void SetFromZ(ref Normal z)
        {
#if VERBOSE
            if (z.Length2 < MathLab.Epsilon)
            {
                Console.WriteLine("z is Zero {1} - {0}",z, z.IsZero());                
                z = new Normal(0,0,1f);
            }
#endif
            Vec3f tmpZ = new Vector(z.x, z.y, z.z);
            Vec3f tmpX = (Math.Abs(tmpZ.x) > 0.99f) ? new Vec3f(0, 1, 0) : new Vec3f(1, 0, 0);
            mY = Vector.Normalize(Core.Primitives.Normal.Cross(ref z, ref tmpX));
            mX = Core.Primitives.Normal.Cross(ref mY, ref z);
            mZ = tmpZ;
        }

        public Vec3f ToWorld(ref Vec3f a)
        {
            return mX * a.x + mY * a.y + mZ * a.z;
        }

        public Vec3f ToLocal(ref Vec3f a)
        {
            return new Vec3f(Vector.Dot(ref a, ref mX), Vector.Dot(ref a, ref mY), Vector.Dot(ref a, ref mZ));
        }

        public Vec3f Binormal() { return mX; }
        public Vec3f Tangent() { return mY; }
        public Vec3f Normal() { return mZ; }

        Vec3f mX, mY, mZ;
    }
}